#ifndef MECANUMDRIVESUBSYSTEM_H
#define MECANUMDRIVESUBSYSTEM_H

// Include files.
#include <boost/smart_ptr/shared_ptr.hpp>
#include "WPILib.h"
#include "AsyncResult.hpp"
#include "RobotSubsystem.hpp"
#include "DriveSubsystem.hpp"
#include "Modifier.hpp"
#include "TwoJoystickDriveController.hpp"
#include "FourMotorTankDriveSubsystem.hpp"
#include "AsyncResultStartState.hpp"
#include "AsyncResultEffectState.hpp"
#include "XboxDriveController.hpp"
#include "Ps3DriveController.hpp"

/**
 *  The MecanumDriveSubsystem class is a class for drive subsystems that use mecanum wheels.
 */

class MecanumDriveSubsystem :
	public FourMotorTankDriveSubsystem
{
	private:
		class DriveDistanceAsyncResult;
	public:
		struct CartesianValue
		{
			double m_x;
			double m_y;
			double m_rotation;
			
			CartesianValue();
			CartesianValue(double x, double y, double rotation);
		};
		
		enum DriveMode
		{
			DriveModeNone  = 0,
			DriveModeCartesian = 1,
			DriveModePolar     = 2,
		};
		
		class DriveStartState;
		class DriveState;
		
		class DriveDistance
		{
			friend class MecanumDriveSubsystem;
			friend class DriveStartState;
			friend class DriveState;
			friend class DriveDistanceAsyncResult;
			
			public:
				DriveDistance();
				DriveDistance(DriveMode mode, double x, double y, double rotation, double threshold);
			private:
				DriveMode m_driveMode;
				double m_x;             // Polar: Magnitude
				double m_y;             // Polor: Direction
				double m_rotation;
				double m_threshold;
				double GetWheelDistance(const MecanumDriveSubsystem& drive, MotorPosition position);
		};
		
		class ScaleModifier :
			public Modifier<CartesianValue>
		{
			public:
				ScaleModifier(double scale);
				virtual void Modify(CartesianValue &value);
			private:
				double m_scale;
		};
		
		class DeadZoneModifier :
			public Modifier<CartesianValue>
		{
			public:
				DeadZoneModifier(double minimumAbsValue);
				virtual void Modify(CartesianValue &value);
			private:
				double m_minimumAbsValue;
		};
		
		class DriveStartState :
			public AsyncResultStartState<bool>
		{
			public:
				DriveStartState(MecanumDriveSubsystem &drive, DriveMode mode, double x, double y, double rotation);
				DriveStartState(MecanumDriveSubsystem &drive, DriveMode mode, double x, double y, double rotation, double threshold);
			protected:
				MecanumDriveSubsystem &m_drive;
				DriveDistance m_distance;
				
				virtual void InitializeImpl();
		};
		
		class DriveState :
			public AsyncResultEffectState<bool>
		{
			public:
				DriveState(MecanumDriveSubsystem &drive, DriveMode mode, double x, double y, double rotation);
				DriveState(MecanumDriveSubsystem &drive, DriveMode mode, double x, double y, double rotation, double treshold);
			private:
				DriveStartState m_stateStart;
		};
		
		static CartesianValue TankDriveWithPassiveMecanumValueFetcher
		(
			GenericHID &leftJoystick, 
			GenericHID &rightJoystick
		);
		
		static CartesianValue TankDriveWithActiveMecanumValueFetcher
		(
			GenericHID &leftJoystick,
			GenericHID &rightJoystick
		);
		
		static CartesianValue ArcadeDriveNormalStyleWithMecanumValueFetcher
		(
			GenericHID &leftJoystick,
			GenericHID &rightJoystick
		);
			
		static CartesianValue ArcadeDriveModernStyleWithMecanumValueFetcher
		(
			GenericHID &leftJoystick,
			GenericHID &rightJoystick
		);
		
		static CartesianValue ArcadeDriveModernStyleWithMecanumValueFetcher
		(
			XboxJoystick &joystick
		);
		
		static CartesianValue ArcadeDriveNormalStyleWithMecanumValueFetcher
		(
			XboxJoystick &joystick
		);
		
		static CartesianValue TankDriveWithPassiveMecanumValueFetcher
		(
			XboxJoystick &joystick
		);
		
		static CartesianValue TankDriveWithPassiveMecanumValueFetcher
		(
			Ps3Joystick &joystick
		);
		
		static CartesianValue ArcadeDriveModernStyleWithMecanumValueFetcher
		(
			Ps3Joystick &joystick
		);
		
		static CartesianValue ArcadeDriveNormalStyleWithMecanumValueFetcher
		(
			Ps3Joystick &joystick
		);
		
		static CartesianValue TiltDriveStyleWithMecanumValueFetcher
		(
			Ps3Joystick &joystick
		);
		
		static void DriveByCartesian
		(
			MecanumDriveSubsystem &drive, 
			CartesianValue &value
		);
		
		MecanumDriveSubsystem
		(
			UINT32 leftFrontMotorChannel, 
			UINT32 leftBackMotorChannel,
			UINT32 rightFrontMotorChannel, 
			UINT32 rightBackMotorChannel
		);
		
		MecanumDriveSubsystem
		(
			CANJaguar *leftFrontMotor, 
			CANJaguar *leftBackMotor,
			CANJaguar *rightFrontMotor, 
			CANJaguar *rightBackMotor
		);
		
		MecanumDriveSubsystem
		(
			CANJaguar *leftFrontMotor, 
			CANJaguar *leftBackMotor,
			CANJaguar *rightFrontMotor, 
			CANJaguar *rightBackMotor,
			Gyro *gyro
		);
		
		MecanumDriveSubsystem
		(
			CANJaguar &leftFrontMotor, 
			CANJaguar &leftBackMotor,
			CANJaguar &rightFrontMotor, 
			CANJaguar &rightBackMotor
		);
		
		MecanumDriveSubsystem
		(
			CANJaguar &leftFrontMotor, 
			CANJaguar &leftBackMotor,
			CANJaguar &rightFrontMotor, 
			CANJaguar &rightBackMotor,
			Gyro &gyro
		);
		
		MecanumDriveSubsystem
		(
			SpeedController *leftFrontMotor, 
			SpeedController *leftBackMotor,
			SpeedController *rightFrontMotor, 
			SpeedController *rightBackMotor
		);
		
		MecanumDriveSubsystem
		(
			SpeedController &leftFrontMotor, 
			SpeedController &leftBackMotor,
			SpeedController &rightFrontMotor, 
			SpeedController &rightBackMotor
		);
		
		MecanumDriveSubsystem
		(
			RobotDrive *drive,
			CANJaguar *leftFrontMotor, 
			CANJaguar *leftBackMotor, 
			CANJaguar *rightFrontMotor,  
			CANJaguar *rightBackMotor
		);
		
		MecanumDriveSubsystem
		(
			RobotDrive &drive,
			CANJaguar &leftFrontMotor, 
			CANJaguar &leftBackMotor, 
			CANJaguar &rightFrontMotor,  
			CANJaguar &rightBackMotor
		);
		
		MecanumDriveSubsystem(RobotDrive *drive);
		MecanumDriveSubsystem(RobotDrive &drive);
		
		virtual ~MecanumDriveSubsystem();
		
		virtual void Stop();
		
		void DriveByCartesian(double x, double y, double rotation);
		void DriveByPolar(double magnitude, double direction, double rotation);
		
		boost::shared_ptr<AsyncResult<bool> > DriveDistanceByCartesian(double x, double y, double rotation);
		boost::shared_ptr<AsyncResult<bool> > DriveDistanceByCartesian(double x, double y, double rotation, double threshold);
		boost::shared_ptr<AsyncResult<bool> > DriveDistanceByPolar(double magnitude, double direction, double rotation);
	protected:
		// These are functions inherited from RobotSubsystem.
		virtual void Periodic();
	private:
		static Gyro &s_nullGyro;
		
		class DriveDistanceAsyncResult :
			public AsyncResult<bool>
		{
			public:
				DriveDistanceAsyncResult(DriveDistance &driveDistance);
				
				virtual bool GetIsFinished() const;
				virtual void Cancel();
				virtual bool GetResult() const;
				
				void Finish(bool result);
			private:
				bool m_isFinished;
				bool m_result;
				DriveDistance &m_driveDistance;
		};
		
		double m_gyroAnglePrevious;
		Gyro &m_gyro;
		DriveDistance m_driveDistance;
		boost::shared_ptr<DriveDistanceAsyncResult> m_driveDistanceAsyncResult;
		
		CartesianValue CompensateWithGyro(CartesianValue value);
		void CancelCurrentDriveDistance();
		void DriveDistanceByCartesianInternal();
		
		void Startup();
		
		
};

typedef TwoJoystickDriveController
<
	MecanumDriveSubsystem,
	MecanumDriveSubsystem::CartesianValue,
	MecanumDriveSubsystem::TankDriveWithPassiveMecanumValueFetcher,
	MecanumDriveSubsystem::DriveByCartesian
>
TankDriveWithPassiveMecanumController;

typedef TwoJoystickDriveController
<
	MecanumDriveSubsystem,
	MecanumDriveSubsystem::CartesianValue,
	MecanumDriveSubsystem::TankDriveWithActiveMecanumValueFetcher,
	MecanumDriveSubsystem::DriveByCartesian
>
TankDriveWithActiveMecanumController;

typedef TwoJoystickDriveController
<
	MecanumDriveSubsystem,
	MecanumDriveSubsystem::CartesianValue,
	MecanumDriveSubsystem::ArcadeDriveNormalStyleWithMecanumValueFetcher,
	MecanumDriveSubsystem::DriveByCartesian
>
ArcadeDriveNormalStyleWithMecanumController;

typedef TwoJoystickDriveController
<
	MecanumDriveSubsystem,
	MecanumDriveSubsystem::CartesianValue,
	MecanumDriveSubsystem::ArcadeDriveModernStyleWithMecanumValueFetcher,
	MecanumDriveSubsystem::DriveByCartesian
>
ArcadeDriveModernStyleWithMecanumController;

typedef XboxDriveController
<
	MecanumDriveSubsystem,
	MecanumDriveSubsystem::CartesianValue,
	MecanumDriveSubsystem::ArcadeDriveModernStyleWithMecanumValueFetcher,
	MecanumDriveSubsystem::DriveByCartesian
>
XboxArcadeDriveModernStyleWithMecanumController;

typedef XboxDriveController
<
	MecanumDriveSubsystem,
	MecanumDriveSubsystem::CartesianValue,
	MecanumDriveSubsystem::ArcadeDriveNormalStyleWithMecanumValueFetcher,
	MecanumDriveSubsystem::DriveByCartesian
>
XboxArcadeDriveNormalStyleWithMecanumController;

typedef XboxDriveController
<
	MecanumDriveSubsystem,
	MecanumDriveSubsystem::CartesianValue,
	MecanumDriveSubsystem::TankDriveWithPassiveMecanumValueFetcher,
	MecanumDriveSubsystem::DriveByCartesian
>
XboxTankDriveWithPassiveMecanumController;

typedef PS3DriveController
<
	MecanumDriveSubsystem,
	MecanumDriveSubsystem::CartesianValue,
	MecanumDriveSubsystem::TankDriveWithPassiveMecanumValueFetcher,
	MecanumDriveSubsystem::DriveByCartesian
>
PS3TankDriveWithPassiveMecanumController;

typedef PS3DriveController
<
	MecanumDriveSubsystem,
	MecanumDriveSubsystem::CartesianValue,
	MecanumDriveSubsystem::ArcadeDriveModernStyleWithMecanumValueFetcher,
	MecanumDriveSubsystem::DriveByCartesian
>
PS3ArcadeDriveModernStyleWithMecanumController;

typedef PS3DriveController
<
	MecanumDriveSubsystem,
	MecanumDriveSubsystem::CartesianValue,
	MecanumDriveSubsystem::ArcadeDriveNormalStyleWithMecanumValueFetcher,
	MecanumDriveSubsystem::DriveByCartesian
>
PS3ArcadeDriveNormalStyleWithMecanumController;

typedef PS3DriveController
<
	MecanumDriveSubsystem,
	MecanumDriveSubsystem::CartesianValue,
	MecanumDriveSubsystem::TiltDriveStyleWithMecanumValueFetcher,
	MecanumDriveSubsystem::DriveByCartesian
>
PS3TiltDriveStyleWithMecanumValueFetcher;

#endif
